#include "panel_teleop.h"
#include "ui_navi_teleop.h"

#include <geometry_msgs/Twist.h>
#include <system_ctrl/algo_ctrl.h>
#include <sensor_msgs/PointField.h>
#include <iostream>  
#include <fstream>
#include <string>  
#include <sstream>
#include <math.h>
#include <boost/filesystem.hpp>
#include <ros/package.h>
#include <QKeyEvent>
#include <QTimer>
#include <QMessageBox>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <ctime> 
#include <chrono>
#include <tf/transform_datatypes.h>

namespace rviz_plugins
{
    bool file_exist(std::string file)
    {
        if (access(file.c_str(), F_OK) != -1) {  
       	    return true;
	} else {
	    return false; 
        } 
    }
    
    std::string trim(const std::string& str) 
    {  
        size_t first = str.find_first_not_of(" \t\n");
        if (first == std::string::npos) return "";
        size_t last = str.find_last_not_of(" \t\n") + 1;
        return str.substr(first, last - first);
    }  

    std::string get_file_content(std::string f)
    {
        std::ifstream file(f);
        if (!file.is_open()) 
        {  
            return "";
        }
        std::string con="";
        std::string line="";
        while (getline(file, line)) 
        {  
            line=trim(line);
            if(!line.empty())
                con=line;
        }
        return con;
    }
    
    template <typename T>
    std::string to_string_with_precision(const T Value, const int Digits = 6)
    {
        std::ostringstream out;
        out.precision(Digits);
        out << std::fixed << Value;
        return out.str();
    }

    TeleopPanel::TeleopPanel(QWidget* Parent/* = nullptr*/)
		: QWidget(Parent), ui_(new Ui::NaviTeleop())
        , node_handle_(std::make_unique<ros::NodeHandle>())
	{
		// set up the GUI
		ui_->setupUi(this);
        // a widget normally must setFocusPolicy() to something other than Qt::NoFocus in order to receive focus events
        setFocusPolicy(Qt::StrongFocus);
        
        // signals
        connect(ui_->start_loc, &QPushButton::clicked, this, [=]() { startOrStopLoc(0); });
        connect(ui_->restart_loc, &QPushButton::clicked, this, [=]() { startOrStopLoc(1); });
        connect(ui_->stop_loc, &QPushButton::clicked, this, [=]() { startOrStopLoc(-1); });
        connect(ui_->start_nav, &QPushButton::clicked, this, [=]() { startOrStopNav(1); });
        connect(ui_->stop_nav, &QPushButton::clicked, this, [=]() { startOrStopNav(0); });
        connect(ui_->start_mapping, &QPushButton::clicked, this, [=]() { startOrStopMapping(1); });
        connect(ui_->stop_mapping, &QPushButton::clicked, this, [=]() { startOrStopMapping(0); });
        connect(ui_->start_robot, &QPushButton::clicked, this, [=]() { startOrStopRobot(1); });
        connect(ui_->stop_robot, &QPushButton::clicked, this, [=]() { startOrStopRobot(0); });
        connect(ui_->start_keyboard, &QPushButton::clicked, this, [=]() { startOrStopKeyboard(1); });
        connect(ui_->stop_keyboard, &QPushButton::clicked, this, [=]() { startOrStopKeyboard(0); });
        connect(ui_->tof_points, &QPushButton::clicked, this, [=]() { pointsSwitch(0); });
        connect(ui_->rdf_points, &QPushButton::clicked, this, [=]() { pointsSwitch(1); });
        connect(ui_->stereo_points, &QPushButton::clicked, this, [=]() { pointsSwitch(2); });
        connect(ui_->reset_points, &QPushButton::clicked, this, [=]() { resetPoints(); });
        connect(ui_->start_record, &QPushButton::clicked, this, [=]() { startOrStopRecord(1); });
        connect(ui_->stop_record, &QPushButton::clicked, this, [=]() { startOrStopRecord(0); });
        
        QStringList qslist;
	qslist.append("background-color:rgb(255,255,255)");
	ui_->start_loc->setStyleSheet(qslist.join(";"));
	ui_->restart_loc->setStyleSheet(qslist.join(";"));
	ui_->start_nav->setStyleSheet(qslist.join(";"));
	ui_->stop_nav->setStyleSheet(qslist.join(";"));
	ui_->start_mapping->setStyleSheet(qslist.join(";"));
	ui_->stop_mapping->setStyleSheet(qslist.join(";"));
	ui_->start_robot->setStyleSheet(qslist.join(";"));
	ui_->stop_robot->setStyleSheet(qslist.join(";"));
	ui_->start_keyboard->setStyleSheet(qslist.join(";"));
	ui_->stop_keyboard->setStyleSheet(qslist.join(";"));
	ui_->tof_points->setStyleSheet(qslist.join(";"));
	ui_->rdf_points->setStyleSheet(qslist.join(";"));
	ui_->stereo_points->setStyleSheet(qslist.join(";"));
        
        start_loc_pub_ = std::make_unique<ros::Publisher>(node_handle_->advertise<system_ctrl::algo_ctrl>("/viobot/stereo2_ctrl", 50));
        twist_pub_ = std::make_unique<ros::Publisher>(node_handle_->advertise<geometry_msgs::Twist>("/cmd_vel", 50));
        recond3d_pub_ = std::make_unique<ros::Publisher>(node_handle_->advertise<sensor_msgs::PointCloud2>("/recon3d", 50));
        tof_sub = node_handle_->subscribe("/viobot/pr_loop/tof_points", 10, &TeleopPanel::tof_callback, this);
        rdf_sub = node_handle_->subscribe("/viobot/pr_loop/rdf_points", 10, &TeleopPanel::rdf_callback, this);
	stereo_sub = node_handle_->subscribe("/viobot/pr_loop/points", 10, &TeleopPanel::stereo_callback, this);
	odom_sub = node_handle_->subscribe("/viobot_odom", 10, &TeleopPanel::odom_callback, this);
	
	boost::filesystem::path path(ros::package::getPath("rviz_plugins"));
	std::string cfg = path.string() + "/configs/url";
	if(file_exist(cfg))
	{
	    std::string con=get_file_content(cfg);
	    if(!con.empty())
	    {
	        ui_->record_path->setText(QString::fromStdString(con));
	    }
	}
	cfg = path.string() + "/configs/topics";
	if(file_exist(cfg))
	{
	    std::string con=get_file_content(cfg);
	    if(!con.empty())
	    {
	        ui_->record_topics->setText(QString::fromStdString(con));
	    }
	}
	
	ui_->record_path->installEventFilter(this);
	ui_->record_topics->installEventFilter(this);

        session = ssh_new(); 
        ssh_options_set(session, SSH_OPTIONS_HOST, "192.168.1.100");
    	ssh_options_set(session, SSH_OPTIONS_USER, "root");
    	ssh_connect(session);
    	ssh_userauth_password(session, "root", "PRR");
    	
    	ssh_channel channel = ssh_channel_new(session);
    	ssh_channel_open_session(channel);
    	std::time_t now = std::time(nullptr);  
    	auto now_s = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
    	std::stringstream ss;  
        ss << now_s;  
        std::string now_s_str = ss.str();
        now_s_str = "cd /home/PRR && nohup bash /home/PRR/sync_time.sh " + now_s_str + " 2>&1 1>/dev/null >nohup.out 2>&1 &"; 
    	int rc = ssh_channel_request_exec(channel, now_s_str.c_str());
        if (rc != SSH_OK) 
        {
            ROS_INFO("和VIOBOT同步时间失败，将导致录制ROSBAG功能无法开启，请手动同步时间!");
            sync_time=false;
        }
        else
        {
            sync_time=true;
        }
        ssh_channel_send_eof(channel);
	ssh_channel_close(channel);
    	ssh_channel_free(channel);
    	
    	th_publish_ = std::thread
        {
            [this]() -> void
            {
                while (ros::ok())
                {
                    if (twist_switch)
                    {
                    	double linear_ = ui_->line_speed->value();
                    	double angular_ = ui_->angle_speed->value();
		        geometry_msgs::Twist msgTwist;
		        msgTwist.linear.x = linear_ * line;
		        msgTwist.angular.z = angular_ * angle;
		        twist_pub_->publish(msgTwist);
                    }
                    std::this_thread::sleep_for(std::chrono::milliseconds(200));
                }
            }
        };
        
        th_publish_2 = std::thread
        {
            [this]() -> void
            {
                while (ros::ok())
                {
                    int pt_type_=pt_type;
                    if(pt_type_==0)
        	    {
        	    	tof_mut.lock();
        	    }
        	    else if(pt_type_==1)
        	   {
        	   	rdf_mut.lock();
        	    }
        	    else
        	    {
        	    	stereo_mut.lock();
        	    }
                    sensor_msgs::PointCloud2 out_pc;
                    ros::Time timestamp = ros::Time::now();
                    out_pc.header.stamp = timestamp;
                    out_pc.header.frame_id = "map";
                    out_pc.height = 1;
                    sensor_msgs::PointField pf_x;
                    sensor_msgs::PointField pf_y;
                    sensor_msgs::PointField pf_z;
                    pf_x.name="x";
                    pf_y.name="y";
                    pf_z.name="z";
                    pf_x.offset=0;
                    pf_y.offset=4;
                    pf_z.offset=8;
                    pf_x.datatype=7;
                    pf_y.datatype=7;
                    pf_z.datatype=7;
                    pf_x.count=1;
                    pf_y.count=1;
                    pf_z.count=1;
                    out_pc.fields.push_back(pf_x);
                    out_pc.fields.push_back(pf_y);
                    out_pc.fields.push_back(pf_z);
                    out_pc.is_bigendian = false;
                    out_pc.point_step = 12;
                    out_pc.is_dense = false;
                    if(pt_type_==0)
                    {
            		out_pc.width = int(tof_vecs.size() / 12);
            		out_pc.row_step = tof_vecs.size();
            		out_pc.data.assign(tof_vecs.begin(), tof_vecs.end());
                    }
        	    else if(pt_type_==1)
        	   {
        	   	out_pc.width = int(rdf_vecs.size()/12);
        	   	out_pc.row_step = rdf_vecs.size();
        	   	out_pc.data.assign(rdf_vecs.begin(), rdf_vecs.end());
        	    }
        	    else
        	    {
        	    	out_pc.width = int(stereo_vecs.size()/12);
        	    	out_pc.row_step = stereo_vecs.size();
        	    	out_pc.data.assign(stereo_vecs.begin(), stereo_vecs.end());
        	    }
        	    recond3d_pub_->publish(out_pc);
        	    if(pt_type_==0)
        	    {
        	    	tof_mut.unlock();
        	    }
        	    else if(pt_type_==1)
        	   {
        	   	rdf_mut.unlock();
        	    }
        	    else
        	    {
        	    	stereo_mut.unlock();
        	    }

                    std::this_thread::sleep_for(std::chrono::milliseconds(200));
                }
            }
        };
    }

    TeleopPanel::~TeleopPanel()
    {
	delete ui_;
    	ssh_disconnect(session);
    	ssh_free(session);
    }
    
    bool TeleopPanel::eventFilter(QObject *obj, QEvent *event)
    {
    	if (event->type() == QEvent::FocusOut) {
    	    QTextEdit *textEdit = qobject_cast<QTextEdit*>(obj);  
            if (textEdit) {
                boost::filesystem::path path(ros::package::getPath("rviz_plugins"));
                std::string cfg;
                std::string txt_con="";
                std::string objectName = textEdit->property("objectName").toString().toStdString();

                if(objectName=="record_path")
                {
                   cfg = path.string() + "/configs/url";
                }
                else if(objectName=="record_topics")
                {
                   cfg = path.string() + "/configs/topics";
                }
                else
                {
                   return false;
                }
                txt_con=trim(textEdit->toPlainText().toStdString());
                
                std::cout<< objectName << std::endl;
                std::cout<< txt_con << std::endl;
	        std::ofstream outFile(cfg);
	        if (outFile.is_open()) {
	            outFile<<txt_con;
                }
                outFile.close();
            }  
        }
        return false;  
    }
    
    void TeleopPanel::odom_callback(const nav_msgs::Odometry &odom_msg)
    {
        double x = odom_msg.pose.pose.position.x;
    	double y = odom_msg.pose.pose.position.y;
    	
    	tf::Quaternion quat;
        tf::quaternionMsgToTF(odom_msg.pose.pose.orientation, quat);
        double roll, pitch, yaw;
        tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
    	
    	std::stringstream ss_x,ss_y,ss_r,ss_p,ss_ya;
    	ss_x << std::setprecision(3) << x;
    	ss_y << std::setprecision(3) << y;
    	ss_r << std::setprecision(3) << roll*57.29578;
    	ss_p << std::setprecision(3) << pitch*57.29578;
    	ss_ya << std::setprecision(3) << yaw*57.29578;
    	
    	std::string sx,sy,sr,sp,sya;
    	sx = ss_x.str(); 
    	sy = ss_y.str();
    	sr = ss_r.str(); 
    	sp = ss_p.str(); 
    	sya = ss_ya.str(); 
    	
    	std::string xyz=sx+", "+sy+", 0.0";
    	std::string rpy=sr+", "+sp+", "+sya;
    	
        ui_->xyz->setText(QString::fromStdString(xyz));
        ui_->rpy->setText(QString::fromStdString(rpy));
    }
    
    void TeleopPanel::tof_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg_in)
    {
    	tof_mut.lock();
        tof_vecs.insert(tof_vecs.end(), cloud_msg_in->data.begin(), cloud_msg_in->data.end());
    	tof_mut.unlock();
    }

    void TeleopPanel::rdf_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg_in)
    {
    	rdf_mut.lock();
        rdf_vecs.insert(rdf_vecs.end(), cloud_msg_in->data.begin(), cloud_msg_in->data.end());
    	rdf_mut.unlock();
    }

    void TeleopPanel::stereo_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg_in)
    {
    	stereo_mut.lock();
        stereo_vecs.insert(stereo_vecs.end(), cloud_msg_in->data.begin(), cloud_msg_in->data.end());
    	stereo_mut.unlock();
    }
    
    void TeleopPanel::startOrStopRecord(int type) 
    {
    	if(type == 1)
    	{
		boost::filesystem::path path(ros::package::getPath("rviz_plugins"));
		std::string path_txt = trim(ui_->record_path->toPlainText().toStdString());
		std::string topics_txt = trim(ui_->record_topics->toPlainText().toStdString());
		if(path_txt.empty() || topics_txt.empty())
		{
			QMessageBox::warning(nullptr, "警告", "录制地址和Topics都不能为空！"); 
			return;
		}
    		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->start_record->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->stop_record->setStyleSheet(qslist2.join(";"));
		std::string cmd = "nohup bash " + path.string() + "/scripts/run_record.sh \""+path_txt+"\" \""+topics_txt+"\" 2>&1 1>/dev/null >nohup.out 2>&1 &";
		system(cmd.c_str());
    	}
    	else
    	{
    		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->stop_record->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->start_record->setStyleSheet(qslist2.join(";"));
		boost::filesystem::path path(ros::package::getPath("rviz_plugins"));
		std::string cmd = "nohup bash " + path.string() + "/scripts/stop_record.sh 2>&1 1>/dev/null >nohup.out 2>&1 &";
		system(cmd.c_str());
    	}
    }
    
    void TeleopPanel::startOrStopKeyboard(int type) 
    {	
	if(type == 1)
	{
		twist_switch=true;
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->start_keyboard->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->stop_keyboard->setStyleSheet(qslist2.join(";"));
	}
	else
	{
		twist_switch=false;
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->stop_keyboard->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->start_keyboard->setStyleSheet(qslist2.join(";"));
	}
    }
    void TeleopPanel::pointsSwitch(int type) 
    {	
	pt_type=type;
	
	if(type == 0)
	{
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->tof_points->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->rdf_points->setStyleSheet(qslist2.join(";"));
		QStringList qslist3;
		qslist3.append("background-color:rgb(255,255,255)");
		ui_->stereo_points->setStyleSheet(qslist3.join(";"));
	}
	else if(type == 1)
	{
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->rdf_points->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->tof_points->setStyleSheet(qslist2.join(";"));
		QStringList qslist3;
		qslist3.append("background-color:rgb(255,255,255)");
		ui_->stereo_points->setStyleSheet(qslist3.join(";"));
	}
	else
	{
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->stereo_points->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->rdf_points->setStyleSheet(qslist2.join(";"));
		QStringList qslist3;
		qslist3.append("background-color:rgb(255,255,255)");
		ui_->tof_points->setStyleSheet(qslist3.join(";"));
	}
    }
    void TeleopPanel::resetPoints() 
    {	
	tof_mut.lock();
	std::vector<uint8_t>().swap(tof_vecs);
	tof_mut.unlock();
	rdf_mut.lock();
	std::vector<uint8_t>().swap(rdf_vecs);
	rdf_mut.unlock();
	stereo_mut.lock();
	std::vector<uint8_t>().swap(stereo_vecs);
	stereo_mut.unlock();
    }
    
    void TeleopPanel::startOrStopRobot(int type) 
    {
    	ssh_channel channel = ssh_channel_new(session);
    	ssh_channel_open_session(channel);
    	if (type==1)
    	{
    		int rc = ssh_channel_request_exec(channel, "nohup bash /home/PRR/run_init.sh 2>&1 1>/dev/null >nohup.out 2>&1 &");
       		if (rc != SSH_OK) 
       	 	{
        		ROS_INFO("底盘启动失败!");
        	}
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->start_robot->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->stop_robot->setStyleSheet(qslist2.join(";"));
    	}
    	else
    	{
        	int rc = ssh_channel_request_exec(channel, "nohup bash /home/PRR/run_stop_init.sh 2>&1 1>/dev/null >nohup.out 2>&1 &");
        	if (rc != SSH_OK) 
        	{
        		ROS_INFO("底盘关闭失败!");
        	}
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->stop_robot->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->start_robot->setStyleSheet(qslist2.join(";"));
    	}
	ssh_channel_send_eof(channel);
	ssh_channel_close(channel);
    	ssh_channel_free(channel);
    }
    
    void TeleopPanel::startOrStopNav(int type) 
    {
    	ssh_channel channel = ssh_channel_new(session);
    	ssh_channel_open_session(channel);
        if(type==1)
        {
        	int rc = ssh_channel_request_exec(channel, "nohup bash /home/PRR/run_navigation.sh 2>&1 1>/dev/null >nohup.out 2>&1 &");
        	if (rc != SSH_OK) 
        	{
        		ROS_INFO("导航开启失败!");
        	}
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->start_nav->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->stop_nav->setStyleSheet(qslist2.join(";"));
        }
        else
        {
        	int rc = ssh_channel_request_exec(channel, "nohup bash /home/PRR/run_stop_navigation.sh 2>&1 1>/dev/null >nohup.out 2>&1 &");
        	if (rc != SSH_OK) 
        	{
        		ROS_INFO("导航关闭失败!");
        	}
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->stop_nav->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->start_nav->setStyleSheet(qslist2.join(";"));
        }
	ssh_channel_send_eof(channel);
	ssh_channel_close(channel);
    	ssh_channel_free(channel);
    }

    void TeleopPanel::startOrStopMapping(int type) 
    {
    	ssh_channel channel = ssh_channel_new(session);
    	ssh_channel_open_session(channel);
        if(type==1)
        {
        	int rc = ssh_channel_request_exec(channel, "nohup bash /home/PRR/run_mapping.sh 2>&1 1>/dev/null >nohup.out 2>&1 &");
        	if (rc != SSH_OK) 
        	{
        		ROS_INFO("建图开启失败!");
        	}
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->start_mapping->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->stop_mapping->setStyleSheet(qslist2.join(";"));
        }
        else
        {
        	int rc = ssh_channel_request_exec(channel, "nohup bash /home/PRR/run_stop_mapping.sh 2>&1 1>/dev/null >nohup.out 2>&1 &");
        	if (rc != SSH_OK) 
        	{
        		ROS_INFO("建图关闭失败!");
        	}
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->stop_mapping->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->start_mapping->setStyleSheet(qslist2.join(";"));
        }
	ssh_channel_send_eof(channel);
	ssh_channel_close(channel);
    	ssh_channel_free(channel);
    }
    
    void TeleopPanel::startOrStopLoc(int type)
    {
        system_ctrl::algo_ctrl msg;
        msg.header.seq = 0;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = "";
        if(type == 0) {
		msg.algo_enable = true;
		msg.algo_reboot = false;
		msg.algo_reset = false;
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->start_loc->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->restart_loc->setStyleSheet(qslist2.join(";"));
		QStringList qslist3;
		qslist3.append("background-color:rgb(255,255,255)");
		ui_->stop_loc->setStyleSheet(qslist3.join(";"));
        }
        if(type == 1) {
		msg.algo_enable = true;
		msg.algo_reboot = true;
		msg.algo_reset = false;
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->restart_loc->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->start_loc->setStyleSheet(qslist2.join(";"));
		QStringList qslist3;
		qslist3.append("background-color:rgb(255,255,255)");
		ui_->stop_loc->setStyleSheet(qslist3.join(";"));
        }
        if(type == -1) {
		msg.algo_enable = false;
		msg.algo_reboot = false;
		msg.algo_reset = false;
		QStringList qslist;
		qslist.append("background-color:rgb(100,200,0)");
		ui_->stop_loc->setStyleSheet(qslist.join(";"));
		QStringList qslist2;
		qslist2.append("background-color:rgb(255,255,255)");
		ui_->restart_loc->setStyleSheet(qslist2.join(";"));
		QStringList qslist3;
		qslist3.append("background-color:rgb(255,255,255)");
		ui_->start_loc->setStyleSheet(qslist3.join(";"));
        }
        start_loc_pub_->publish(msg);
    }
    
    void TeleopPanel::keyPressEvent(QKeyEvent* Event)
    {
        switch (Event->key())
        {
        case Qt::Key_Left:
            angle=1;
            break;
        case Qt::Key_Right:
            angle=-1;
            break;
        case Qt::Key_Up:
            line=1;
            break;
        case Qt::Key_Down:
            line=-1;
            break;
        default:
            break;
        }
    }

    void TeleopPanel::keyReleaseEvent(QKeyEvent* Event)
    {
        switch (Event->key())
        {
        case Qt::Key_Left:
            angle=0;
            break;
        case Qt::Key_Right:
            angle=0;
            break;
        case Qt::Key_Up:
            line=0;
            break;
        case Qt::Key_Down:
            line=0;
            break;
        default:
            break;
        }
    }
} // end namespace rviz_plugins
